#include <Encoder.h>
#include <SoftwareSerial.h>
#include <serLCD.h>

#include "tweaks.h"
#include "commandParser.h";
//=========================================================================================================
void setup()
{
	pinSetup();
	accSetup();

	Serial.begin(9600);
	LCD.setBrightness(15);

	LCD.clear();
	LCD.selectLine(1);
	LCD.print("Welcome to FH");
	// initializing of the last read temperature so the alarm does not go off from the start
	CURRENT_TEMPERATURE = TEMPERATURE_OFFSET + getCelsius(analogRead(NTC_PIN));	
	LAST_READ_TEMPERATURE  = CURRENT_TEMPERATURE;
	MAXIMUM_TEMPERATURE = CURRENT_TEMPERATURE;
	MINIMUM_TEMPERATURE = CURRENT_TEMPERATURE;
	LAST_AUTOMATIC_TEMPERATURE_TRANSMITED = CURRENT_TEMPERATURE;
}
//=========================================================================================================
void loop()
{
	readData();
	if (millis() - LCD_LAST_REFRESH > LCD_REFRESH_INTERVAL) {
		LCD.clear();
		printData();
	}
	readCommands();
	checkAlarms();
 }
//=========================================================================================================
int getState()
{
	int mod = (ENC_VALUE % STATES_COUNT + 
		((ENC_VALUE < 0) ? STATES_COUNT : 0)) % STATES_COUNT;
	return mod;
}
//=========================================================================================================
void checkAlarms()
{
	float accX = getAcc('x');
	float accY = getAcc('y');
	float accZ = getAcc('z');
	// Accelerometer =========== alarms
	float totalAccSquare = accX*accX+accY*accY+accZ*accZ;	
	if (totalAccSquare > TOTAL_ACCELERATION_ALARM_THRESHOLD*TOTAL_ACCELERATION_ALARM_THRESHOLD)
		digitalWrite(YELLOW_ALARM_LED1, HIGH);
	else
		digitalWrite(YELLOW_ALARM_LED1, LOW);
	
	// ========= Temperature alarms
	// sudden change in temperature alarm check 
	if (millis() - TIME_SINCE_LAST_READ_TEMPERATURE > TIME_BETWEEN_TEMPERATURE_READINGS) 
	{
		float deltaTemperature = LAST_READ_TEMPERATURE-CURRENT_TEMPERATURE;
		// check for (delta temperature / delta Time )bigger than threshold // using square cause abs function is not working propertly
		if  (deltaTemperature*deltaTemperature > SUDEN_CHANGE_TEMPERATURE_THRESHOLD*SUDEN_CHANGE_TEMPERATURE_THRESHOLD)
			digitalWrite(YELLOW_ALARM_LED2, HIGH);
		else
			digitalWrite(YELLOW_ALARM_LED2, LOW);				
		TIME_SINCE_LAST_READ_TEMPERATURE = millis();
		LAST_READ_TEMPERATURE  = CURRENT_TEMPERATURE;		
	}
	// transmision of temperature if it passes over defined threshold
	if (CURRENT_TEMPERATURE > AUTOMATIC_TEMPERATURE_TRANSMISION_THRESHOLD	 
		&& AUTOMATIC_TEMPERATURE_TRANSMISION_PERIOD != 0
		&& millis() - TIME_SINCE_LAST_TRANSMITED_TEMPERATURE > AUTOMATIC_TEMPERATURE_TRANSMISION_PERIOD*1000) 
	{
		Message theMessage;
		Message* aMessage = &theMessage;
		aMessage->address 	 = 1;
		aMessage->command 	 = COMMAND_RESPONSE_TEMPERATURE;
		aMessage->dataFormat = 1;
		String dataString = doubleToString(CURRENT_TEMPERATURE,100);
		dataString.toCharArray((char*)aMessage->data,100);
		aMessage->length  	 = sizeof(dataString) + 6;
		sendMessage(aMessage);
		TIME_SINCE_LAST_TRANSMITED_TEMPERATURE = millis();
		LAST_AUTOMATIC_TEMPERATURE_TRANSMITED = CURRENT_TEMPERATURE;
	}

	// transmision of temperature if it changes over a definide diferential since las transmition 
	// using square values cause abs is not working
	if ( (CURRENT_TEMPERATURE  - LAST_AUTOMATIC_TEMPERATURE_TRANSMITED)*(CURRENT_TEMPERATURE  - LAST_AUTOMATIC_TEMPERATURE_TRANSMITED) 
			> AUTOMATIC_TEMPERATURE_TRANSMISION_DIFERENTIAL*AUTOMATIC_TEMPERATURE_TRANSMISION_DIFERENTIAL) 
	{
		Message theMessage;
		Message* aMessage = &theMessage;
		aMessage->address 	 = 1;
		aMessage->command 	 = COMMAND_RESPONSE_TEMPERATURE;
		aMessage->dataFormat = 1;
		String dataString = doubleToString(CURRENT_TEMPERATURE,100);
		dataString.toCharArray((char*)aMessage->data,100);
		aMessage->length  	 = sizeof(dataString) + 6;
		sendMessage(aMessage);
		TIME_SINCE_LAST_TRANSMITED_TEMPERATURE = millis();
		LAST_AUTOMATIC_TEMPERATURE_TRANSMITED = CURRENT_TEMPERATURE;
	}
	// unplugged temperature sensor alarm check 
	if ( CURRENT_TEMPERATURE  > 55.0) 
		digitalWrite(RED_ALARM_LED1, HIGH);
	else
		digitalWrite(RED_ALARM_LED1, LOW);

	// unplugged temperature sensor alarm check 
	if ( ACCELERATION_X  > 5.3 || ACCELERATION_Y  > 5.3 || ACCELERATION_Z  > 5.3) 
		digitalWrite(RED_ALARM_LED2, HIGH);
	else
		digitalWrite(RED_ALARM_LED2, LOW);
		
	
	// temperature out of range alarm check
	if (CURRENT_TEMPERATURE > UPPER_TEMPERATURE_THRESHOLD
		|| CURRENT_TEMPERATURE < LOWER_TEMPERATURE_THRESHOLD)
		digitalWrite(YELLOW_ALARM_LED3, HIGH);
	else
		digitalWrite(YELLOW_ALARM_LED3, LOW);
}
//=========================================================================================================
bool readData()
{
	encUpdate();
	CURRENT_TEMPERATURE = TEMPERATURE_OFFSET + getCelsius(analogRead(NTC_PIN));
	ACCELERATION_X = getAcc('x');
	ACCELERATION_Y = getAcc('y');
	ACCELERATION_Z = getAcc('z');
	
	int s = getState();
	if (s == ACCELERATION_CURRENT) {
		ACCELERATION_X = getAcc('x');
		ACCELERATION_Y = getAcc('y');
		ACCELERATION_Z = getAcc('z');
	} else if (s == ACCELERATION_MIN) {
		ACCELERATION_X = getAcc('x');
		ACCELERATION_Y = getAcc('y');
		ACCELERATION_Z = getAcc('z');
		if (ENC_BTN_VALUE == HIGH) {
			MINIMUM_ACCELERATION_X = ACCELERATION_X;
			MINIMUM_ACCELERATION_Y = ACCELERATION_Y;
			MINIMUM_ACCELERATION_Z = ACCELERATION_Z;
		}
	} else if (s == ACCELERATION_MAX) {
		ACCELERATION_X = getAcc('x');
		ACCELERATION_Y = getAcc('y');
		ACCELERATION_Z = getAcc('z');
		if (ENC_BTN_VALUE == HIGH) {
			MAXIMUM_ACCELERATION_X = ACCELERATION_X;
			MAXIMUM_ACCELERATION_Y = ACCELERATION_Y;
			MAXIMUM_ACCELERATION_Z = ACCELERATION_Z;
		}
	} else if (s == ACCELERATION_ROT) 
	{
		PITCH 	= getRotation('x');
		ROLL	= getRotation('y');
		YAW 	= getRotation('z');
	} else if (s == ACCELERATION_DIST) {
		
	} else if (s == TEMP) {
		// CURRENT_TEMPERATURE = TEMPERATURE_OFFSET + getCelsius(analogRead(NTC_PIN));
	} else if (s == TEMP_RECORD) {
		// CURRENT_TEMPERATURE = TEMPERATURE_OFFSET + getCelsius(analogRead(NTC_PIN));
		// Reset values if button is pressed
		if (ENC_BTN_VALUE == HIGH) {
			Serial.println("+================================");
			Serial.println(CURRENT_TEMPERATURE);				
		
			MAXIMUM_TEMPERATURE = CURRENT_TEMPERATURE;
			MINIMUM_TEMPERATURE = CURRENT_TEMPERATURE;
		}
	}
}
//=========================================================================================================
void printData()
{
	LCD_LAST_REFRESH = millis();

	LCD.selectLine(1);
	int s = getState();
	if (s == ACCELERATION_CURRENT) {
		String acc_x = "Acc X: " + toStr(ACCELERATION_X);
		String acc_y = "Y:" + toStr(ACCELERATION_Y, 2, 1) + " ";
		String acc_z = "Z:" + toStr(ACCELERATION_Z, 2, 1);
		LCD.print(acc_x);
		LCD.selectLine(2);
		LCD.print(acc_y);
		LCD.setCursor(2, 9);
		LCD.print(acc_z);
	} else if (s == ACCELERATION_MIN) {
		String acc_x = "Acc min X:" + toStr(MINIMUM_ACCELERATION_X);
		String acc_y = "Y:" + toStr(MINIMUM_ACCELERATION_Y, 2, 1) + " ";
		String acc_z = "Z:" + toStr(MINIMUM_ACCELERATION_Z, 2, 1);
		LCD.print(acc_x);
		LCD.selectLine(2);
		LCD.print(acc_y);
		LCD.setCursor(2, 9);
		LCD.print(acc_z);
	} else if (s == ACCELERATION_MAX) {
		String acc_x = "Acc max X:" + toStr(MAXIMUM_ACCELERATION_X);
		String acc_y = "Y:" + toStr(MAXIMUM_ACCELERATION_Y, 2, 1) + " ";
		String acc_z = "Z:" + toStr(MAXIMUM_ACCELERATION_Z, 2, 1);
		LCD.print(acc_x);
		LCD.selectLine(2);
		LCD.print(acc_y);
		LCD.setCursor(2, 9);
		LCD.print(acc_z);
	} else if (s == ACCELERATION_ROT) {
		String pitch = "Rot: P:" + toStr(PITCH);
		String roll = "R:" + toStr(ROLL, 2, 1) + " ";
		String yaw = "Y:" + toStr(YAW, 2, 1);
		LCD.print(pitch);
		LCD.selectLine(2);
		LCD.print(roll);
		LCD.setCursor(2, 9);
		LCD.print(yaw);
	} else if (s == ACCELERATION_DIST) {
		LCD.print("Distance:");
	} else if (s == TEMP) {
		LCD.print("Current temp:");
		LCD.selectLine(2);
		LCD.print(toStr(CURRENT_TEMPERATURE));
	} else if (s == TEMP_RECORD) {
		String temp_min = "T min: " + toStr(MINIMUM_TEMPERATURE);
		LCD.print(temp_min);
		LCD.selectLine(2);
		String temp_max = "T max: " + toStr(MAXIMUM_TEMPERATURE);
		LCD.print(temp_max);
	} else if (s == STOP_UPDATE) {
		LCD.print("Welcome to FH");
	} else { 
		LCD.print("Wrong state");
	}
}

